#include "WPILib.h"
#include "Robot1045.h"
#include "Defs.h"

SashaBot::SashaBot(void){
	m_rightMotor = new Jaguar(SIDECAR, RIGHTMOTOR);
	m_leftMotor = new Jaguar(SIDECAR, LEFTMOTOR);
	
	m_robotDrive = new RobotDrive(m_rightMotor, m_leftMotor);
	m_station = DriverStation::GetInstance();
	m_screen =  DriverStationLCD::GetInstance();
	
	m_rightStick = new Joystick(RIGHTSTICK);
	m_leftStick = new Joystick(LEFTSTICK);
	
	GyroInit();
	
	

}

void SashaBot::RobotInit(){
	m_screen->PrintfLine(DriverStationLCD::kUser_Line1, "Hello Dave, hi");
	m_screen->UpdateLCD();
}

void SashaBot::RobotDisabled(){
	m_robotDrive->TankDrive(0.0, 0.0);
	m_screen->PrintfLine(DriverStationLCD::kUser_Line2, "Really? Disabled?");
	m_screen->UpdateLCD();
}

void SashaBot::UpdateSpeed(){
	float oldcrouch;
	oldcrouch = m_crouch;
	GetCrouch();
	
	if(oldcrouch != m_crouch){
		m_screen->PrintfLine(DriverStationLCD::kUser_Line2,"The m_crouch = %f",  m_crouch );
		m_screen->UpdateLCD();
		cout << "The m_crouch = " <<  m_crouch << endl;
	}

	m_rightValue = SpeedControl(m_rightStick->GetY())*m_crouch;
	m_leftValue = SpeedControl(m_leftStick->GetY())*m_crouch;


	
}

double SashaBot::SpeedControl(float pos){
	double speed = 0;
	
	if (pos >= -0.06 && pos <= 0.06){
		return 0.0;
	}

	if  (pos > 0){
		speed =  1.0 / (1.0 + 250.0 * std::exp(-10.0 * pos));
	}
	if(pos <0){
		speed = -1.0 / (1.0 + 250.0 * std::exp( 10.0 * pos));
		return speed;
	}
	
	if(speed>= 1){
		speed = 1;
		return speed;
	}
	
	return speed;
	
}

void SashaBot::GetCrouch(){
	m_crouch = m_rightStick->GetZ();
		
	m_crouch = 1 - m_crouch;
	m_crouch = m_crouch/2;

}

void SashaBot::GyroInit(){
	m_gyro = new Gyro(GYRO);
	m_gyro->Reset();
	m_angle = m_gyro->GetAngle();
	m_accelrometer = new ADXL345_I2C(SIDECAR);
}

void SashaBot::GyroControl(){
	m_angle = m_gyro->GetAngle(); 
	m_screen->PrintfLine(DriverStationLCD::kUser_Line5, "Angle: 0.1%f", m_angle);
	m_screen->UpdateLCD();
}

void SashaBot::AccelControl(){
	
	double tempx = m_accelrometer->GetAcceleration(ADXL345_I2C::kAxis_X);
	double tempy = m_accelrometer->GetAcceleration(ADXL345_I2C::kAxis_Y);
	double tempz = m_accelrometer->GetAcceleration(ADXL345_I2C::kAxis_Z);
//	cout << "x accel " << tempx;
//	cout << "y accel " << tempy;
//	cout << "z accel " << tempz << endl;
	tempx = floor(tempx*10)/10;
	tempy = floor(tempy*10)/10;
	tempz = floor(tempz*10)/10;
	
	if(current_x != tempx){
		cout << "x accel " << tempx << endl;
		current_x = tempx;
	}
	if(current_y != tempy){
			cout << "y accel " << tempy << endl;
			current_y = tempy;
		}
	
	if(current_z != tempz){
			cout << "z accel " << tempz << endl;
			current_z = tempz;
		}
}



START_ROBOT_CLASS(SashaBot);

